Collaborative 3-D localization of robots with heterogeneous inter-robot relative measurements

We propose a method for collaborative localization of multiple autonomous robots that fuses relative-pose measurements of each robot with various types of inter-vehicle relative measurements. We do not rely on the use of any maps, or the ability to recognize landmarks in the environment. Instead we assume that noisy relative measurements between pairs of robots which can be one of any of the following: pose (position and orientation), orientation, position, bearing, distance are intermittently available. We utilize the additional information about each robot’s pose provided by these measurements to improve over self-localization (dead-reckoning) estimates. The proposed method is based on solving an optimization problem in a product manifold. A provably correct explicit gradient descent law on the manifold is provided. Unlike many previous approaches, the proposed algorithm is easily applicable to the 3-D case. The distributed algorithm we propose is also capable of handling a fully dynamic scenario where the neighbor relationships among robots are time-varying. Simulations as well as experiments with a pair of ground robots show that the errors in the localization estimates obtained using this algorithm are significantly lower than what is achieved when robots estimate their poses without cooperation. Additionally, the improvement of localization over dead reckoning for the various types of measurements considered allows us to examine cost-benefit tradeoffs of the sensors required to obtain such measurements.